#include "stm32f4xx_aes.h"
#include "aes_impl.h"
#include <string.h>

/* AES寄存器结构体 */
typedef struct
{
    __IO uint32_t CR;      /* 控制寄存器 */
    __IO uint32_t SR;      /* Status寄存器 */
    __IO uint32_t DINR;    /* DataInput寄存器 */
    __IO uint32_t DOUTR;   /* DataOutput寄存器 */
    __IO uint32_t KEYR0;   /* Key寄存器0 */
    __IO uint32_t KEYR1;   /* Key寄存器1 */
    __IO uint32_t KEYR2;   /* Key寄存器2 */
    __IO uint32_t KEYR3;   /* Key寄存器3 */
    __IO uint32_t IVR0;    /* InitializeVector寄存器0 */
    __IO uint32_t IVR1;    /* InitializeVector寄存器1 */
    __IO uint32_t IVR2;    /* InitializeVector寄存器2 */
    __IO uint32_t IVR3;    /* InitializeVector寄存器3 */
} AES_TypeDef;

/* AES外设基地址 */
#define AES_BASE            ((uint32_t)0x50060000)
#define AES                 ((AES_TypeDef *) AES_BASE)

/* 内部变量 */
static uint8_t aes_key[AES_KEY_SIZE];
static uint8_t aes_iv[AES_BLOCK_SIZE];
static uint8_t internal_password[32];  // 内部口令
static uint8_t password_set = 0;       // 口令YesNo已Settings
static AES_Impl_Context aes_ctx;       // 标准AES上下文

/* 标准AES-128实现（Useaes_implModule） */
static void software_aes_encrypt(uint8_t* input, uint8_t* output)
{
    // Use标准AES-128 ECB模式加密Single块
    AES_Impl_Encrypt_Block(&aes_ctx, input, output);
}

static void software_aes_decrypt(uint8_t* input, uint8_t* output)
{
    // Use标准AES-128 ECB模式解密Single块
    AES_Impl_Decrypt_Block(&aes_ctx, input, output);
}

void AES_DeInit(void)
{
    // DisableAES外设
    // Note：InQEMU中Skip硬件访问，Use软件实现
    // AES->CR = 0;
}

void AES_Init(void)
{
    // InitializeAES（软件实现，Not访问硬件）
    // AES_DeInit();
    
    // SettingsDefaultKeyAndIV
    uint8_t default_key[16] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
                               0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
    uint8_t default_iv[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    
    AES_SetKey(default_key);
    AES_SetIV(default_iv);
    
    // 清除内部口令
    memset(internal_password, 0, sizeof(internal_password));
    password_set = 0;
}

void AES_Enable(void)
{
    AES->CR |= AES_CR_EN;
}

void AES_Disable(void)
{
    AES->CR &= ~AES_CR_EN;
}

void AES_SetKey(uint8_t* key)
{
    if (key != NULL) {
        memcpy(aes_key, key, AES_KEY_SIZE);
        // 重新InitializeAES上下文
        AES_Impl_Init(&aes_ctx, aes_key, aes_iv);
    }
}

void AES_SetIV(uint8_t* iv)
{
    if (iv != NULL) {
        memcpy(aes_iv, iv, AES_BLOCK_SIZE);
        // 重新InitializeAES上下文
        AES_Impl_Init(&aes_ctx, aes_key, aes_iv);
    }
}

void AES_Encrypt(uint8_t* plaintext, uint8_t* ciphertext, uint32_t length)
{
    uint32_t blocks = (length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE;
    
    for (uint32_t i = 0; i < blocks; i++) {
        uint8_t input_block[AES_BLOCK_SIZE];
        uint8_t output_block[AES_BLOCK_SIZE];
        
        // 准备Input块
        memset(input_block, 0, AES_BLOCK_SIZE);
        uint32_t block_size = (i == blocks - 1) ? (length % AES_BLOCK_SIZE) : AES_BLOCK_SIZE;
        if (block_size == 0) block_size = AES_BLOCK_SIZE;
        
        memcpy(input_block, plaintext + i * AES_BLOCK_SIZE, block_size);
        
        // 加密
        software_aes_encrypt(input_block, output_block);
        
        // 复制Output
        memcpy(ciphertext + i * AES_BLOCK_SIZE, output_block, AES_BLOCK_SIZE);
    }
}

void AES_Decrypt(uint8_t* ciphertext, uint8_t* plaintext, uint32_t length)
{
    uint32_t blocks = (length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE;
    
    for (uint32_t i = 0; i < blocks; i++) {
        uint8_t input_block[AES_BLOCK_SIZE];
        uint8_t output_block[AES_BLOCK_SIZE];
        
        // 准备Input块
        memcpy(input_block, ciphertext + i * AES_BLOCK_SIZE, AES_BLOCK_SIZE);
        
        // 解密
        software_aes_decrypt(input_block, output_block);
        
        // 复制Output
        uint32_t block_size = (i == blocks - 1) ? (length % AES_BLOCK_SIZE) : AES_BLOCK_SIZE;
        if (block_size == 0) block_size = AES_BLOCK_SIZE;
        
        memcpy(plaintext + i * AES_BLOCK_SIZE, output_block, block_size);
    }
}

void AES_SetPassword(uint8_t* password, uint32_t length)
{
    if (password != NULL && length > 0) {
        memset(internal_password, 0, sizeof(internal_password));
        uint32_t copy_length = (length > sizeof(internal_password) - 1) ? 
                              sizeof(internal_password) - 1 : length;
        memcpy(internal_password, password, copy_length);
        password_set = 1;
    }
}

void AES_ProcessCommand(uint8_t cmd, uint8_t* data, uint32_t length, uint8_t* response, uint32_t* response_length)
{
    *response_length = 0;
    
    switch (cmd) {
        case AES_CMD_SET_PASSWORD:
            if (length > 0) {
                AES_SetPassword(data, length);
                response[0] = 0x00;  // SuccessStatus
                *response_length = 1;
            } else {
                response[0] = 0x01;  // ErrorStatus
                *response_length = 1;
            }
            break;
            
        case AES_CMD_ENCRYPT:
            if (length > 0) {
                // 响应Format: [Status][Ciphertext长度][CiphertextData]
                uint8_t* plaintext = data;
                uint32_t plaintext_length = length;
                
                // CalculateCiphertext长度（按块Alignment）
                uint32_t ciphertext_length = ((plaintext_length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;
                
                // 分配临时缓冲区
                uint8_t temp_ciphertext[256];  // False设最大256字节
                if (ciphertext_length <= sizeof(temp_ciphertext)) {
                    AES_Encrypt(plaintext, temp_ciphertext, plaintext_length);
                    
                    response[0] = 0x00;  // SuccessStatus
                    response[1] = (ciphertext_length >> 8) & 0xFF;  // Ciphertext长度高字节
                    response[2] = ciphertext_length & 0xFF;         // Ciphertext长度低字节
                    memcpy(response + 3, temp_ciphertext, ciphertext_length);
                    *response_length = 3 + ciphertext_length;
                } else {
                    response[0] = 0x02;  // Data过长Error
                    *response_length = 1;
                }
            } else {
                response[0] = 0x01;  // ErrorStatus
                *response_length = 1;
            }
            break;
            
        case AES_CMD_DECRYPT:
            if (length > 0) {
                // 响应Format: [Status][Plaintext长度][PlaintextData]
                uint8_t* ciphertext = data;
                uint32_t ciphertext_length = length;
                
                // CalculatePlaintext长度（按块Alignment）
                uint32_t plaintext_length = ((ciphertext_length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;
                
                // 分配临时缓冲区
                uint8_t temp_plaintext[256];  // False设最大256字节
                if (plaintext_length <= sizeof(temp_plaintext)) {
                    AES_Decrypt(ciphertext, temp_plaintext, ciphertext_length);
                    
                    response[0] = 0x00;  // SuccessStatus
                    response[1] = (plaintext_length >> 8) & 0xFF;  // Plaintext长度高字节
                    response[2] = plaintext_length & 0xFF;         // Plaintext长度低字节
                    memcpy(response + 3, temp_plaintext, plaintext_length);
                    *response_length = 3 + plaintext_length;
                } else {
                    response[0] = 0x02;  // Data过长Error
                    *response_length = 1;
                }
            } else {
                response[0] = 0x01;  // ErrorStatus
                *response_length = 1;
            }
            break;
            
        default:
            response[0] = 0x03;  // 未知CommandError
            *response_length = 1;
            break;
    }
}
